/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.circuitrunners.frc2011.arm;

import com.circuitrunners.frc2011.*;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import com.sun.squawk.util.MathUtils;

/**
 *
 * @author Anupam and James
 */
public class Arm extends Components implements AutoArm {
    private final int lesser = -1, equal = 0, greater = 1;
    private AnalogChannel thetaChannel, alphaChannel;
    private Victor jointOne, jointTwo;
    private double alpha, theta, beta;
    private final double D = 61, B = 48.5, C = 45;
    private final double MAX_ANGLE = 5* Math.PI / 3;
    private  AxisCamera cam;
    private Servo cameraServo;
    public static final double radiansToDegrees = 180/Math.PI, y = 53;
    public void run(){
        Watchdog.getInstance().feed();
        //alpha = alphaChannel.getValue() * MAX_ANGLE;
        //theta = thetaChannel.getValue() * MAX_ANGLE;
            //if(B*Math.sin(alpha)+(C)*(Math.cos(theta - alpha - Math.PI / 2)) < D){
                jointOne.set(getXBOX_AXIS(2)); //Theta Change----
                jointTwo.set(getXBOX_AXIS(4)); //Alpha Change
            }// else {
               /* switch(switchValues(alpha, Math.PI/2)){
                    case lesser:
                        atLimit(true);
                        break;
                    case equal:
                        switch(switchValues(theta, Math.PI)){
                            case lesser:
                                atLimit(false);
                                break;
                            case equal:
                                throw new RuntimeException("Impossible Case Assumed");
                            case greater:
                                atLimit(true);
                                break;
                        }
                        break;
                    case greater:
                        atLimit(false);
                        break;
                }
            }
        } catch (Throwable t){

        }
        cameraServo.setAngle(radiansToDegrees * MathUtils.atan(
                (-Math.cos(alpha)+Math.sin((Math.PI/2)+alpha+theta)+y)
                                      /
                (Math.sin(alpha)+Math.cos((Math.PI/2)+alpha-theta))));
    */

    private void atLimit(boolean bool) throws Throwable {
        double dTheta = getXBOX_AXIS(2);
        double dAlpha = getXBOX_AXIS(4);
        if(bool ? dAlpha < 0 : dAlpha > 0)
            jointTwo.set(dAlpha);
        if(bool ? dTheta > 0 : dTheta < 0)
            jointOne.set(dTheta);
    }
    private void atLimit(boolean bool, double speed, double newTheta, double newAlpha) throws Throwable {
        if(bool){
            if(newAlpha < alpha)
                jointTwo.set(-speed);
            if(newTheta > theta)
                jointOne.set(speed);
        } else {
            if(newAlpha > alpha)
                jointTwo.set(speed);
            if(newTheta < theta)
                jointOne.set(-speed);
        }
    }
    private int switchValues(final double input, final double threshold){
        if(input < threshold)
            return lesser;
        else if(input > threshold)
            return greater;
        else
            return equal;
    }
    public void init() {
        //thetaChannel = new AnalogChannel(1,1);
        //alphaChannel = new AnalogChannel(1,2);
        jointOne = new Victor(4,2);
        jointTwo = new Victor(4,3);
        //cam = AxisCamera.getInstance();
    }
    public void setArm(double newTheta, double newAlpha){
        final double speed = 1.0;
        //alpha = alphaChannel.getValue() * MAX_ANGLE;
        //theta = thetaChannel.getValue()* MAX_ANGLE;
        try{
            if(B*Math.sin(alpha)+(C)*(Math.cos(theta - alpha - Math.PI / 2)) < D){
                switch(switchValues(theta, newTheta)){
                    case lesser: jointOne.set(-speed); break;
                    case equal: jointOne.set(0); break;
                    case greater: jointOne.set(speed); break;
                }
                switch(switchValues(alpha, newAlpha)){
                    case lesser: jointTwo.set(-speed); break;
                    case equal: jointTwo.set(0); break;
                    case greater: jointTwo.set(speed); break;
                }
            } else {
                switch(switchValues(alpha, Math.PI/2)){
                    case lesser:
                        atLimit(true, speed, newTheta, newAlpha);
                        break;
                    case equal:
                        switch(switchValues(theta, Math.PI)){
                            case lesser:
                                atLimit(false, speed, newTheta, newAlpha);
                                break;
                            case equal:
                                throw new RuntimeException("Impossible Case Assumed");
                            case greater:
                                atLimit(true, speed, newTheta, newAlpha);
                                break;
                        }
                        break;
                    case greater:
                        atLimit(false, speed, newTheta, newAlpha);
                        break;
                }
            }
        } catch (Throwable ex){
           //ex.printStackTrace();
        }
    }

    public void disable() {
        try{
            jointOne.set(0);
            jointTwo.set(0);
        } catch (Throwable t){
            //t.printStackTrace();
        }
    }

}
